/*
 * @Description:
 * @Version: 2.0
 * @Autor: ruog__
 * @Date: 2024-03-06 13:19:37
 * @LastEditors: ruog__
 * @LastEditTime: 2024-03-06 16:40:20
 */
#include "mapwidget.h"
#include <QPainter>
#include <QTimer>
#include <cmath>
#include <QtSql>
#include <QDebug>


MapWidget::MapWidget(QWidget *parent): QWidget(parent)
{
    setFixedSize(480, 480);
    vison_ros_string = "初始化完成";

    samePathCount = 0;
    carOldPositon.x = 0;
    carOldPositon.y = 0;



    QTimer *timer = new QTimer(this);
    connect(timer, &QTimer::timeout, this, &MapWidget::moveCar);
    timer->start(10); // 每1秒移动一次

    // forwardButton = new QPushButton(QString::fromUtf8("启动"), this);
    // forwardButton->setGeometry(20, 520, 80, 30);
    // connect(forwardButton, &QPushButton::clicked, this, &MapWidget::startCar);

    // backwardButton = new QPushButton(QString::fromUtf8("后退"), this);
    // backwardButton->setGeometry(120, 520, 80, 30);
    // connect(backwardButton, &QPushButton::clicked, this, &MapWidget::backward);

    // leftButton = new QPushButton(QString::fromUtf8("左转"), this);
    // leftButton->setGeometry(220, 520, 80, 30);
    // connect(leftButton, &QPushButton::clicked, this, &MapWidget::turnLeft);

    // rightButton = new QPushButton(QString::fromUtf8("右转"), this);
    // rightButton->setGeometry(320, 520, 80, 30);
    // connect(rightButton, &QPushButton::clicked, this, &MapWidget::turnRight);

    // stopButton = new QPushButton(QString::fromUtf8("停止"), this);
    // stopButton->setGeometry(420, 520, 80, 30);
    // connect(stopButton, &QPushButton::clicked, this, &MapWidget::stop);

    boundXz = 0;
    boundXy = 120* (width() / 120);
    boundYs = 0;
    boundYx = 120 * (height() / 120);

    //车库位置初始化
    initPositionX = 0;
    initPositionY = 0;
    xuangengStat = 0;//0:旋耕空闲 1：旋耕工作
    carX = 0;
    carY = 0;
    carDirection = 0;
    carStat = 4; //停止状态

    zuoshang.x = 0;
    zuoshang.y = 0;
    youshang.x = 60 * (width() / 120);
    youshang.y = 0 * (height() / 120);
    zuoxia.x = 0;
    zuoxia.y = 40 * (height() / 120);
    youxia.x = 60 * (width() / 120);
    youxia.y = 40 * (height() / 120);

    transzuobiao();

    visonLog = 0;

    engnge = 100;//电量初始化为100%
}

void MapWidget::transzuobiao()
{
    boundXz = zuoshang.x;
    boundXy = youshang.x;
    boundYs = zuoshang.y;
    boundYx = youxia.y;
}

void MapWidget::gengdi()
{
    double nextX = carX + cos(carDirection * M_PI / 180);
    double nextY = carY + sin(carDirection * M_PI / 180);

    // 判断是否超出范围
    if (nextX < boundXz || nextX >= boundXy || nextY < boundYs || nextY >= boundYx)
    {
        if (nextX < boundXz || nextX >= boundXy)
        {
            nextX = carX;
            if (carDirection == 180)
            {
                carDirection = 270; // 转向上
                // boundYx -= 5;
                // nextY = carY -= 4.1;
                xuangengStat = 1;//旋耕工作
            }
            else if (carDirection == 0)
            {
                carDirection = 90; // 转向下
                // boundYs += 5;
                // nextY = carY += 4.1;
                xuangengStat = 1;//旋耕工作
            }
            else
            {
                // 其他角度说明变成手动控制。
            }
        }

        if (nextY < boundYs || nextY >= boundYx)
        {
            nextY = carY;
            if (carDirection == 90)
            {
                carDirection = 180;//转向左
                boundXy -= 5;
                nextX = carX -= 3.1;
                xuangengStat = 0;//旋耕空闲
            }
            else if (carDirection == 270)
            {
                carDirection = 0;//转向右
                boundXz += 5;
                nextX = carX += 5.1;
                xuangengStat = 0;//旋耕空闲
            }
            else
            {

            }
        }
    }
    else
    {
        carX = nextX;
        carY = nextY;
    }

    if(carOldPositon.x == carX && carOldPositon.y == carY)
    {
        samePathCount++;
        if(samePathCount >= 10)
        {
            carStat = 3;//离开耕地
            carDirection = 180;//方向向左回左上角
            xuangengStat = 0;//旋耕空闲
        }
    }
    carOldPositon.x = carX;
    carOldPositon.y = carY;
    trackPoints.push_back(QPoint(static_cast<int>(carX), static_cast<int>(carY)));
    update();
}

void MapWidget::fanhuichejian()
{
    double nextX = carX + cos(carDirection * M_PI / 180);
    double nextY = carY + sin(carDirection * M_PI / 180);

    // 判断是否超出范围
    if (nextX < 0 || nextX >= youshang.x || nextY < 0 || nextY >= zuoxia.y)
    {
        if (nextX < 0 || nextX >= zuoshang.x)
        {
            nextX = carX;
            if (carDirection == 180)
            {
                carDirection = 0;
                carX = 0;
                carY = 0;
                carStat = 4;//空闲状态
                engnge = 100;//电量在初始点充电完成
                xuangengStat = 0;//关闭旋耕机
                emit mySignal();//发射信号保存图片
                trackPoints.clear();
            }
            else if (carDirection == 0)
            {

            }
            else
            {
                // 其他角度说明变成手动控制。
            }
        }

        if (nextY < 0 || nextY >= zuoshang.y)
        {
            nextY = carY;
            if (carDirection == 270)//到耕耘初始点
            {
                carDirection = 180;
            }
            else
            {
            }
        }
    }
    else
    {
        carX = nextX;
        carY = nextY;
    }
    trackPoints.push_back(QPoint(static_cast<int>(carX), static_cast<int>(carY)));
    update();
}

void MapWidget::likaigengdi()
{
    double nextX = carX + cos(carDirection * M_PI / 180);
    double nextY = carY + sin(carDirection * M_PI / 180);

    // 判断是否超出范围
    if (nextX < zuoshang.x || nextX >= youshang.x || nextY < zuoshang.y || nextY >= zuoxia.y)
    {
        if (nextX < zuoshang.x || nextX >= zuoshang.x)
        {
            nextX = carX;
            if (carDirection == 180)
            {
                carStat = 5;//进入到返回车间状态
                carDirection = 270;//方向转向上
                carX = zuoshang.x;
                carY = zuoshang.y;
            }
            else if (carDirection == 0)
            {

            }
            else
            {
                // 其他角度说明变成手动控制。
            }
        }

        if (nextY < zuoshang.y || nextY >= zuoxia.y)
        {
            nextY = carY;
            if (carDirection == 270)//到耕耘初始点
            {
                carDirection = 180;
            }
            else
            {
            }
        }
    }
    else
    {
        carX = nextX;
        carY = nextY;
    }
    trackPoints.push_back(QPoint(static_cast<int>(carX), static_cast<int>(carY)));
    update();
}

void MapWidget::qugengdi()
{
    double nextX = carX + cos(carDirection * M_PI / 180);
    double nextY = carY + sin(carDirection * M_PI / 180);

    // 判断是否超出范围
    if (nextX < 0 || nextX >= zuoshang.x || nextY < 0 || nextY >= zuoshang.y)
    {
        if (nextX < 0 || nextX >= zuoshang.x)
        {
            nextX = carX;
            if (carDirection == 180)
            {
                carDirection = 270; // 转向上
                boundYx -= 5;
                nextY = carY -= 5.1;
            }
            else if (carDirection == 0)
            {
                carDirection = 90; // 转向下
            }
            else
            {
                // 其他角度说明变成手动控制。
            }
        }

        if (nextY < 0 || nextY >= zuoshang.y)
        {
            nextY = carY;
            if (carDirection == 90)//到耕耘初始点
            {
                carDirection = 0;
                carX = zuoshang.x;
                carY = zuoshang.y;
                carStat = 1;//转到耕耘状态
            }
            else
            {
                carDirection = 0;//转向右
                boundXz += 5;
                nextX = carX += 5.1;
            }
        }
    }
    else
    {
        carX = nextX;
        carY = nextY;
    }
    trackPoints.push_back(QPoint(static_cast<int>(carX), static_cast<int>(carY)));
    update();
}


void MapWidget::updateLog(QString log)
{

}

void MapWidget::write_sql()
{
    QDateTime currentDateTime = QDateTime::currentDateTime();
    QString localTime = currentDateTime.toString("yyyy-MM-dd hh:mm:ss");
    QString stationid = "1";
    QString name1 = "test";

    QSqlQuery query;
    query.prepare("INSERT INTO position2 (day0, stationid, name1, x1, y1) VALUES (:val1, :val2, :val3, :val4, :val5)");
    query.bindValue(":val1", localTime);
    query.bindValue(":val2", stationid);
    query.bindValue(":val3", name1);
    query.bindValue(":val4", QString::number(carX));
    query.bindValue(":val5", QString::number(carY));

    if (!query.exec())
    {
        qDebug() << "Insertion failed:" << query.lastError().text();
    }
}

void MapWidget::moveCar()
{
    if (stopped != 1)
    {
        if(engnge <= 20 && carStat != 3)//电量低，需要充电
        {
            carStat = 3;//离开耕地，回去充电
            carDirection = 270;//方向向上
            xuangengStat = 0;//旋耕空闲
            emit mySignal();//发射信号保存图片
        }
        if(engnge <= 0)//电量耗尽停机
        {
            carStat = 4;//车辆关闭
            xuangengStat = 0;//旋耕机关闭
            emit mySignal();//发射信号保存图片
        }
        if(visonLog++ % 401 == 0)
            write_sql();

        //0:空闲 1.耕耘  2.去耕耘  3.返回车间 4:主车停止状态
        switch (carStat)
        {
        case 0://空闲
            if(xuangengStat != 0)//旋耕不空闲时
                engnge -= 0.01;
            engnge -= 0.001;//空闲状态电量减0.001%
            break;
        case 1://耕耘中
            if(xuangengStat == 1)
                engnge -= 0.02; //旋耕打开时，电量每次减去0.02%
            else
                engnge -= 0.01; //旋耕关闭时，电量每次减去0.01%
            gengdi();
            break;
        case 2://去耕耘
            if(xuangengStat != 0)//旋耕不空闲时
                engnge -= 0.01;
            engnge -= 0.01; //车辆运行时，电量每次减去0.01%
            qugengdi();
            break;
        case 3://返回zuoshangjiao
            if(xuangengStat != 0)//旋耕不空闲时
                engnge -= 0.01;
            engnge -= 0.01; //车辆运行时，电量每次减去0.01%
            likaigengdi();
            break;
        case 4://主车停止状态不损耗电量
            if(xuangengStat != 0)//旋耕不空闲时
                engnge -= 0.01;
            break;
        case 5://返回车间
            if(xuangengStat != 0)//旋耕不空闲时
                engnge -= 0.01;
            fanhuichejian();
            break;
        default:
            break;
        }
    }
}

void MapWidget::startCar()
{
    stopped = false;
    carStat = 1; //耕地
}

void MapWidget::backward()
{
    double step = 5.0;
    double dx = step * cos((carDirection + 180) * M_PI / 180.0);
    double dy = step * sin((carDirection + 180) * M_PI / 180.0);
    carX += dx;
    carY += dy;

    trackPoints.push_back(QPoint(static_cast<int>(carX), static_cast<int>(carY)));

    update();
}

void MapWidget::turnLeft()
{
    carDirection += 10;
    carDirection = fmod(carDirection, 360);
    update();
}

void MapWidget::turnRight()
{
    carDirection -= 10;
    carDirection = fmod(carDirection, 360);
    update();
}

void MapWidget::stop()
{
    stopped = true;
}
